#include "../../../XMLTools.h"
#include "../../../MathTools.h"

#include "Force1DSensorMeasurement.h"

using namespace MMM;

Force1DSensorMeasurement::Force1DSensorMeasurement(float timestep, float force, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type),
    force(force)
{
}

SensorMeasurementPtr Force1DSensorMeasurement::clone() {
    return clone(timestep);
}

bool Force1DSensorMeasurement::equals(SensorMeasurementPtr sensorMeasurement) {
    Force1DSensorMeasurementPtr ptr = boost::dynamic_pointer_cast<Force1DSensorMeasurement>(sensorMeasurement);
    if (ptr) {
        return Math::equal(force, ptr->force);
    }
    return false;
}

Force1DSensorMeasurementPtr Force1DSensorMeasurement::clone(float newTimestep) {
    Force1DSensorMeasurementPtr clonedSensorMeasurement(new Force1DSensorMeasurement(newTimestep, force, type));
    return clonedSensorMeasurement;
}

void Force1DSensorMeasurement::appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode) {
    measurementNode->append_node("Force")->append_data_node(XML::toString(force));
}

Force1DSensorMeasurementPtr Force1DSensorMeasurement::interpolate(Force1DSensorMeasurementPtr other, float timestep, SensorMeasurementType type) {
    float interpolatedForce = Math::linearInterpolation(this->force, this->timestep, other->force, other->timestep, timestep);
    Force1DSensorMeasurementPtr interpolatedSensorMeasurement(new Force1DSensorMeasurement(timestep, interpolatedForce, type));
    return interpolatedSensorMeasurement;
}

float Force1DSensorMeasurement::getForce() {
    return force;
}
